
#include <BMesh>
#include "IGjkEpaSolver.h"
#include "ISoftBodyInternals.h"
#include "member_BSoftBody.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

void Joint::Prepare(BReal dt, int)
{
	bodies[0].body->activate();
	bodies[1].body->activate();
}

void LJoint::Prepare(BReal dt, int iterations)
{
	static const BReal maxdrift = 4;
	Joint::Prepare(dt, iterations);
	rpos[0] = bodies[0].xform() * refs[0];
	rpos[1] = bodies[1].xform() * refs[1];
	drift = Clamp(rpos[0] - rpos[1], maxdrift) * factor / dt;
	rpos[0] -= bodies[0].xform().position();
	rpos[1] -= bodies[1].xform().position();
	massmatrix = ImpulseMatrix(
		bodies[0].invMass(), bodies[0].invInertia(), rpos[0],
		bodies[1].invMass(), bodies[1].invInertia(), rpos[1]);
	if (split > 0)
	{
		sdrift = massmatrix * drift * split;
		drift *= 1 - split;
	}
	drift /= (BReal)iterations;
}
void LJoint::Solve(BReal dt, BReal sor)
{
	const BVector va = bodies[0].velocity(rpos[0]);
	const BVector vb = bodies[1].velocity(rpos[1]);
	const BVector vr = va - vb;
	Impulse impulse;
	impulse.asVelocity = 1;
	impulse.velocity = massmatrix * drift + vr * forceMixing * sor;
	bodies[0].applyImpulse(-impulse, rpos[0]);
	bodies[1].applyImpulse(impulse, rpos[1]);
}
void LJoint::terminate()
{
	if (split > 0)
	{
		bodies[0].applyDImpulse(-sdrift, rpos[0]);
		bodies[1].applyDImpulse(sdrift, rpos[1]);
	}
}

void AJoint::Prepare(BReal dt, int iterations)
{
	static const BReal maxdrift = PI / 16;
	icontrol->Prepare(this);
	Joint::Prepare(dt, iterations);
	axis[0] = bodies[0].xform().mult3(refs[0]);
	axis[1] = bodies[1].xform().mult3(refs[1]);
	drift = NormalizeAny(axis[1].cross(axis[0]));
	drift *= bMin(maxdrift, acos(Clamp(axis[0].dot(axis[1]), -1, +1)));
	drift *= factor / dt;
	massmatrix = AngularImpulseMatrix(bodies[0].invInertia(), bodies[1].invInertia());
	if (split > 0)
	{
		sdrift = massmatrix * drift * split;
		drift *= 1 - split;
	}
	drift /= (BReal)iterations;
}
void AJoint::Solve(BReal dt, BReal sor)
{
	const BVector va = bodies[0].angularVelocity();
	const BVector vb = bodies[1].angularVelocity();
	const BVector vr = va - vb;
	const BReal sp = vr.dot(axis[0]);
	const BVector vc = vr - axis[0] * icontrol->Speed(this, sp);
	Impulse impulse;
	impulse.asVelocity = 1;
	impulse.velocity = massmatrix * drift + vc * forceMixing * sor;
	bodies[0].applyAImpulse(-impulse);
	bodies[1].applyAImpulse(impulse);
}
void AJoint::terminate()
{
	if (split > 0)
	{
		bodies[0].applyDAImpulse(-sdrift);
		bodies[1].applyDAImpulse(sdrift);
	}
}

void CJoint::Prepare(BReal dt, int iterations)
{
	Joint::Prepare(dt, iterations);
	if (life == 0)
	{
		drift = drift * factor / dt;
		if (split > 0)
		{
			sdrift = massmatrix * drift * split;
			drift *= 1 - split;
		}
		drift /= (BReal)iterations;
	}
	else
	{
		drift = sdrift = BVector();
	}
}
void CJoint::Solve(BReal dt, BReal sor)
{
	const BVector va = bodies[0].velocity(rpos[0]);
	const BVector vb = bodies[1].velocity(rpos[1]);
	const BVector vrel = va - vb;
	const BReal rvac = vrel.dot(normal);
	Impulse impulse;
	impulse.asVelocity = 1;
	impulse.velocity = drift;
	if (rvac < 0)
	{
		const BVector iv = normal * rvac;
		const BVector fv = vrel - iv;
		impulse.velocity += iv + fv * friction;
	}
	impulse.velocity = massmatrix * impulse.velocity * sor;

	if (bodies[0].cluster == bodies[1].cluster)
	{
		if ((impulse.velocity.x() == impulse.velocity.x()) && (impulse.velocity.y() == impulse.velocity.y()) &&
			(impulse.velocity.z() == impulse.velocity.z()))
		{
			if (impulse.asVelocity)
			{
				if (impulse.velocity.length() < bodies[0].cluster->maxSelfCollisionImpulse)
				{
				}
				else
				{
					bodies[0].applyImpulse(-impulse * bodies[0].cluster->selfCollisionImpulseFactor, rpos[0]);
					bodies[1].applyImpulse(impulse * bodies[0].cluster->selfCollisionImpulseFactor, rpos[1]);
				}
			}
		}
	}
	else
	{
		bodies[0].applyImpulse(-impulse, rpos[0]);
		bodies[1].applyImpulse(impulse, rpos[1]);
	}
}
void CJoint::terminate()
{
	if (split > 0)
	{
		bodies[0].applyDImpulse(-sdrift, rpos[0]);
		bodies[1].applyDImpulse(sdrift, rpos[1]);
	}
}

void BWE::ClusterVImpulse(Cluster* cluster, const BVector& rpos, const BVector& impulse)
{
	const BVector li = impulse * cluster->imass;
	const BVector ai = cluster->invInertia * rpos.cross(impulse);
	cluster->vimpulses[0] += li;
	cluster->lv += li;
	cluster->vimpulses[1] += ai;
	cluster->av += ai;
	cluster->nvimpulses++;
}
void BWE::ClusterDImpulse(Cluster* cluster, const BVector& rpos, const BVector& impulse)
{
	const BVector li = impulse * cluster->imass;
	const BVector ai = cluster->invInertia * rpos.cross(impulse);
	cluster->dimpulses[0] += li;
	cluster->dimpulses[1] += ai;
	cluster->ndimpulses++;
}
void BWE::ClusterVAImpulse(Cluster* cluster, const BVector& impulse)
{
	const BVector ai = cluster->invInertia * impulse;
	cluster->vimpulses[1] += ai;
	cluster->av += ai;
	cluster->nvimpulses++;
}
void BWE::ClusterDAImpulse(Cluster* cluster, const BVector& impulse)
{
	const BVector ai = cluster->invInertia * impulse;
	cluster->dimpulses[1] += ai;
	cluster->ndimpulses++;
}
void BWE::ClusterAImpulse(Cluster* cluster, const Impulse& impulse)
{
	if (impulse.asVelocity)
		ClusterVAImpulse(cluster, impulse.velocity);
	if (impulse.asDrift)
		ClusterDAImpulse(cluster, impulse.drift);
}
void BWE::ClusterDCImpulse(Cluster* cluster, const BVector& impulse)
{
	cluster->dimpulses[0] += impulse * cluster->imass;
	cluster->ndimpulses++;
}

Body::Body()
{

}
Body::Body(Cluster* p)
{
	cluster = p;
}
Body::Body(BBody* body)
{
	cluster = 0;
	this->body = body;
}

BVector Body::invInertia() const
{
	if (body)
		return body_member(body)->invInertia;
	if (cluster)
		return cluster->invInertia;
	return BVector(1, 1, 1);
}
BReal Body::invMass() const
{
	if (body)
		return body_member(body)->invMass;
	if (cluster)
		return cluster->imass;
	return 0;
}
const BMatrix& Body::xform() const
{
	static BMatrix identity;
	if (body)
		return body->matrix();
	if (cluster)
		return cluster->framexform;
	return identity;
}
BVector Body::linearVelocity() const
{
	if (body)
		return body->linearVelocity();
	if (cluster)
		return cluster->lv;
	return BVector();
}
BVector Body::angularVelocity(const BVector& rpos) const
{
	if (body)
		return body->angularVelocity().cross(rpos);
	if (cluster)
		return cluster->av.cross(rpos);
	return BVector();
}
BVector Body::angularVelocity() const
{
	if (body)
		return body->angularVelocity();
	if (cluster)
		return cluster->av;
	return BVector();
}
BVector Body::velocity(const BVector& rpos) const
{
	return (linearVelocity() + angularVelocity(rpos));
}
void Body::applyVImpulse(const BVector& impulse, const BVector& rpos) const
{
	if (body)
		body->applyImpulse(impulse, rpos);
	if (cluster)
		ClusterVImpulse(cluster, rpos, impulse);
}
void Body::applyDImpulse(const BVector& impulse, const BVector& rpos) const
{
	if (body)
		body->applyImpulse(impulse, rpos);
	if (cluster)
		ClusterDImpulse(cluster, rpos, impulse);
}
void Body::applyImpulse(const Impulse& impulse, const BVector& rpos) const
{
	if (impulse.asVelocity)
	{
		applyVImpulse(impulse.velocity, rpos);
	}
	if (impulse.asDrift)
	{
		applyDImpulse(impulse.drift, rpos);
	}
}
void Body::applyVAImpulse(const BVector& impulse) const
{
	if (body)
		body->applyTorqueImpulse(impulse);
	if (cluster)
		ClusterVAImpulse(cluster, impulse);
}
void Body::applyDAImpulse(const BVector& impulse) const
{
	if (body)
		body->applyTorqueImpulse(impulse);
	if (cluster)
		ClusterDAImpulse(cluster, impulse);
}
void Body::applyAImpulse(const Impulse& impulse) const
{
	if (impulse.asVelocity)
		applyVAImpulse(impulse.velocity);
	if (impulse.asDrift)
		applyDAImpulse(impulse.drift);
}
void Body::applyDCImpulse(const BVector& impulse) const
{
	if (body)
		body->applyCentralImpulse(impulse);
	if (cluster)
		ClusterDCImpulse(cluster, impulse);
}

member_BSoftBody::member_BSoftBody(BSoftBody* softBody)
{
	boss = softBody;

	worldInfo = 0;

	aero_model = eAeroModel::V_Point;
	vel_factor = 1;
	drag_coefficient = 0;
	lift_coefficient = 0;
	pressure_coefficient = 0;
	volume_coefficient = 0;
	soft_hardness = (BReal)1.0;
	anchor_hardness = (BReal)0.99;
	kSRHR_CL = (BReal)0.1;
	kSKHR_CL = (BReal)1;
	kSSHR_CL = (BReal)0.5;
	kSR_SPLT_CL = (BReal)0.5;
	kSK_SPLT_CL = (BReal)0.5;
	kSS_SPLT_CL = (BReal)0.5;
	maxvolume = (BReal)1;

	clusterSolverCount = 4;

	pose.isframe = false;
	pose.space = 0;
	pose.matching = 0;

	timeacc = 0;
	dirty = true;

	sleepingThreshold = BReal(0.04);
	selfCollision = false;
	maxSpeedSquared = 0;
	repulsionStiffness = (BReal)0.5;
	face_dbvnt = 0;
	reducedModel = false;


}
member_BSoftBody::~member_BSoftBody()
{
	releaseClusters();
	for (int i = 0; i < joints.size(); ++i)
		free(joints[i]);
}

void member_BSoftBody::appendLink(int node0, int node1, bool bcheckexist)
{
	if (!bcheckexist || !boss->checkLink(node0, node1))
	{
		Link& link = links.append();
		link.vertex0 = &vertexs[node0];
		link.vertex1 = &vertexs[node1];
		link.length = (link.vertex0->position - link.vertex1->position).length();
		dirty = true;
	}
}
void member_BSoftBody::appendFace(int node0, int node1, int node2)
{
	if (node0 == node1)
		return;
	if (node1 == node2)
		return;
	if (node2 == node0)
		return;
	Face& face = faces.append();
	face.vertex0 = &vertexs[node0];
	face.vertex1 = &vertexs[node1];
	face.vertex2 = &vertexs[node2];
	face.area = AreaOf(face.vertex0->position, face.vertex1->position, face.vertex2->position);
	dirty = true;
}

void member_BSoftBody::updateSpaceTree()
{
	const BReal margin = boss->shape(0)->margin();

	vertex_tree.clear();
	for (int i = 0; i < vertexs.size(); ++i)
	{
		Vertex& vertex = vertexs[i];
		BSpace space(vertex.position, margin);
		SpaceNode* node = vertex_tree.insert(space, &vertex);
		vertex.leaf = node;
		vertex.index = i;
	}

	face_tree.clear();
	for (int i = 0; i < faces.size(); ++i)
	{
		Face& face = faces[i];
		BSpace space = SpaceOf(face, 0);
		SpaceNode* node = face_tree.insert(space, &face);
		face.leaf = node;
	}

	if (!face_tree.empty())
	{
		updateFace(face_tree.root(), false, false);
	}
}
void member_BSoftBody::updateConstants()
{
	for (int i = 0; i < links.size(); ++i)
	{
		Link& link = links[i];
		link.length = (link.vertex0->position - link.vertex1->position).length();
		link.length2 = link.length * link.length;
	}

	for (int i = 0; i < faces.size(); i++)
	{
		Face& face = faces[i];
		face.area = AreaOf(face.vertex0->position, face.vertex1->position, face.vertex2->position);
	}

	BArray<int> counts(vertexs.size());
	for (int i = 0; i < vertexs.size(); i++)
	{
		vertexs[i].area = 0;
	}
	for (int i = 0; i < faces.size(); i++)
	{
		Face& face = faces[i];
		int index0 = (int)(face.vertex0 - vertexs.data());
		counts[index0]++;
		face.vertex0->area += fabs(face.area);

		int index1 = (int)(face.vertex1 - vertexs.data());
		counts[index1]++;
		face.vertex1->area += fabs(face.area);

		int index2 = (int)(face.vertex2 - vertexs.data());
		counts[index2]++;
		face.vertex2->area += fabs(face.area);
	}
	for (int i = 0; i < vertexs.size(); i++)
	{
		if (counts[i] > 0)
			vertexs[i].area /= (BReal)counts[i];
		else
			vertexs[i].area = 0;
	}
}
void member_BSoftBody::updateFace(SpaceNode* node, bool use_velocity, bool use_margin)
{
	if (node->next)
	{
		updateFace(node->prev, use_velocity, use_margin);
		updateFace(node->next, use_velocity, use_margin);
		node->space = node->prev->space | node->next->space;
	}
	else
	{
		Face* face = node->value;
		BReal margin = use_margin ? radmrg : (REAL_EPSILON * 100);
		if (use_velocity)
		{
			BVector points[6] = {
				face->vertex0->position, face->vertex0->position + face->vertex0->velocity * dtime,
				face->vertex1->position, face->vertex1->position + face->vertex1->velocity * dtime,
				face->vertex2->position, face->vertex2->position + face->vertex2->velocity * dtime
			};
			node->space.reset(points[0]);
			node->space.expand(points[1]);
			node->space.expand(points[2]);
			node->space.expand(points[3]);
			node->space.expand(points[4]);
			node->space.expand(points[5]);
		}
		else
		{
			BVector points[3] = { face->vertex0->position, face->vertex1->position, face->vertex2->position };
			node->space.reset(points[0]);
			node->space.expand(points[1]);
			node->space.expand(points[2]);
		}
		node->space.min() -= margin;
		node->space.max() += margin;
	}
}

void member_BSoftBody::evaluateMedium(const BVector& x, Medium& medium)
{
	medium.velocity.set(0, 0, 0);
	medium.pressure = 0;
	medium.density = worldInfo->air_density;
	if (worldInfo->water_density > 0)
	{
		const BReal depth = -(x.dot(worldInfo->water_normal) + worldInfo->water_offset);
		if (depth > 0)
		{
			medium.density = worldInfo->water_density;
			medium.pressure = depth * worldInfo->water_density * (boss->gravity()* boss->mass()).length();
		}
	}
}
bool member_BSoftBody::proximityTest(const BVector& x1, const BVector& x2, const BVector& x3, const BVector& x4, const BVector& normal, const BReal& mrg, BVector& bary)
{
	BVector x43 = x4 - x3;
	if (std::abs(x43.dot(normal)) > mrg)
		return false;
	BVector x13 = x1 - x3;
	BVector x23 = x2 - x3;
	BReal a11 = x13.length2();
	BReal a22 = x23.length2();
	BReal a12 = x13.dot(x23);
	BReal b1 = x13.dot(x43);
	BReal b2 = x23.dot(x43);
	BReal det = a11 * a22 - a12 * a12;
	if (det < REAL_EPSILON)
		return false;
	BReal w1 = (b1 * a22 - b2 * a12) / det;
	BReal w2 = (b2 * a11 - b1 * a12) / det;
	BReal w3 = 1 - w1 - w2;
	BReal delta = mrg / std::sqrt(BReal(0.5) * std::abs(x13.cross(x23).length()));
	bary = BVector(w1, w2, w3);
	for (int i = 0; i < 3; ++i)
	{
		if (bary[i] < -delta || bary[i] > 1 + delta)
			return false;
	}
	return true;
}
BVector member_BSoftBody::generateUnitOrthogonalVector(const BVector& u)
{
	BReal ux = u.x();
	BReal uy = u.y();
	BReal uz = u.z();
	BReal ax = std::abs(ux);
	BReal ay = std::abs(uy);
	BReal az = std::abs(uz);
	BVector v;
	if (ax <= ay && ax <= az)
		v = BVector(0, -uz, uy);
	else if (ay <= ax && ay <= az)
		v = BVector(-uz, 0, ux);
	else
		v = BVector(-uy, ux, 0);
	v.normalize();
	return v;
}

bool member_BSoftBody::checkContact(BBody* body, BShape* shape, const BMatrix& matrix, const BVector& point, BReal margin, ContactInfo& cinfo) const
{
	BVector normal;
	BReal dist = worldInfo->sparsesdf.evaluate(matrix.inverse() * point, shape, normal, margin);
	if (dist < 0)
	{
		cinfo.body = body;
		cinfo.normal = matrix.mult3(normal);
		cinfo.offset = -cinfo.normal.dot(point - cinfo.normal * dist);
		return true;
	}
	return false;
}
bool member_BSoftBody::checkContact(BBody* body, BShape* shape, const BMatrix& matrixB, Face& face, BVector& point, BVector& bary, BReal margin, ContactInfo& cinfo) const
{
	BMatrix triangle_matrix(face.vertex0->prepos);

	ITriangleShape triangleShape(BVector(), face.vertex1->prepos - face.vertex0->prepos, face.vertex2->prepos - face.vertex0->prepos);
	BShape* convexShape = dynamic_cast<BShape*>(shape);

	BVector guess;
	IGjkEpaSolver::Result result;
	IGjkEpaSolver::SignedDistance(&triangleShape, triangle_matrix, convexShape, matrixB, guess, result);
	BReal offset = result.distance - 2 * convexShape->margin() - margin;
	if (offset >= 0)
		return false;

	ITriangleShape triangle2(BVector(), face.vertex1->position - face.vertex0->position, face.vertex2->position - face.vertex0->position);
	triangle_matrix.setPosition(face.vertex0->position);
	IGjkEpaSolver::SignedDistance(&triangle2, triangle_matrix, convexShape, matrixB, guess, result);
	point = result.pointA;

	BVector v0 = face.vertex1->position - face.vertex0->position;
	BVector v1 = face.vertex2->position - face.vertex0->position;
	BVector v2 = point - face.vertex0->position;
	BReal d00 = v0.dot(v0);
	BReal d01 = v0.dot(v1);
	BReal d11 = v1.dot(v1);
	BReal d20 = v2.dot(v0);
	BReal d21 = v2.dot(v1);
	BReal denom = d00 * d11 - d01 * d01;
	if (std::isnan(denom) || fabs(denom) < REAL_EPSILON)
	{
		bary.y() = 0;
		bary.z() = 0;
	}
	else
	{
		bary.y() = (d11 * d20 - d01 * d21) / denom;
		bary.z() = (d00 * d21 - d01 * d20) / denom;
	}
	bary.x() = BReal(1) - bary.y() - bary.z();

	face.baryPlane.setNormal(bary);
	face.baryPlane.setDist(0);

	offset = result.distance - convexShape->margin() - margin;
	cinfo.body = body;
	cinfo.normal = result.normal;
	cinfo.offset = offset;
	return true;
}

bool member_BSoftBody::checkLink(const Vertex* vertex0, const Vertex* vertex1) const
{
	for (int i = 0; i < links.size(); i++)
	{
		const Link& link = links[i];
		if ((link.vertex0 == vertex0 && link.vertex1 == vertex1) || (link.vertex0 == vertex1 && link.vertex1 == vertex0))
		{
			return true;
		}
	}
	return false;
}

void member_BSoftBody::collisionBody(Vertex* vertex, BBody* body, BShape* shape, const BMatrix& matrix, BReal margin)
{
	ContactInfo cinfo;
	if (this->checkContact(body, shape, matrix, vertex->position, margin, cinfo))
	{
		const BReal imassA = vertex->imass;
		const BReal imassB = body_member(body)->invMass;
		const BReal imsum = imassA + imassB;
		if (imsum > 0)
		{
			const BMatrix& matrix = node_member(body)->matrix;
			const BVector& invInertia = body_member(body)->invInertia;
			const BVector localPoint = vertex->position - matrix.position();
			const BVector va = body->velocityInPoint(localPoint) * dtime;
			const BVector vb = vertex->position - vertex->prepos;
			const BVector vr = vb - va;
			const BReal dn = vr.dot(cinfo.normal);
			const BVector fv = vr - cinfo.normal * dn;
			const BReal friction = body->friction();
			VertexContact& contact = vertexContacts.append();
			contact.info = cinfo;
			contact.vertex = vertex;
			contact.impulseMatrix = ImpulseMatrix(dtime, imassA, imassB, invInertia, localPoint);
			contact.localPoint = localPoint;
			contact.friction = fv.length2() < (dn * friction * dn * friction) ? 0 : 1 - friction;
			contact.hardness = body_member(body)->mass > REAL_EPSILON ? 1 : 0.1;
			body->activate();
		}
	}
}
void member_BSoftBody::collisionBody(Face* face, BBody* body, BShape* shape, const BMatrix& matrix, BReal margin)
{
	Vertex* v0 = face->vertex0;
	Vertex* v1 = face->vertex1;
	Vertex* v2 = face->vertex2;
	BVector point;
	BVector bary;
	ContactInfo cinfo;
	if (this->checkContact(body, shape, matrix, *face, point, bary, margin, cinfo))
	{
		BReal imassA = v0->imass + v1->imass + v2->imass;
		const BReal imassB = body_member(body)->invMass;
		const BReal sumims = imassA + imassB;
		if (sumims > 0)
		{
			FaceRigidContact& contact = faceRigidContacts.append();
			contact.info = cinfo;
			contact.point = point;
			contact.bary = bary;
			contact.weight = bary * (2 / (1 + bary.length2()));
			contact.face = face;

			BReal imass = bary.x() * contact.weight.x() * v0->imass + bary.y() * contact.weight.y() * v1->imass + bary.z() * contact.weight.z() * v2->imass;
			contact.imass = imass;
			contact.friction = body_member(body)->friction;
			contact.hardness = body_member(body)->mass > REAL_EPSILON ? 1 : 0.1;
			contact.imassMatrix = Diagonal(imass);
			if (body)
			{
				const BMatrix& matrix = node_member(body)->matrix;
				const BVector& invInertiaTensor = body_member(body)->invInertia;
				contact.localPoint = point - matrix.position();
				contact.impulseMatrix = ImpulseMatrix(1, imass, imassB, invInertiaTensor, contact.localPoint);
			}
		}
	}
}

void member_BSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* bodyA, Body bodyB)
{
	LJoint* pj = new LJoint();
	pj->bodies[0] = bodyA;
	pj->bodies[1] = bodyB;
	pj->refs[0] = pj->bodies[0].xform().inverse().mult3(specs.position);
	pj->refs[1] = pj->bodies[1].xform().inverse().mult3(specs.position);
	pj->forceMixing = specs.forceMixing;
	pj->factor = specs.factor;
	pj->split = specs.split;
	joints.append(pj);
}
void member_BSoftBody::appendLinearJoint(const LJoint::Specs& specs, Body body)
{
	appendLinearJoint(specs, clusters[0], body);
}
void member_BSoftBody::appendLinearJoint(const LJoint::Specs& specs, BSoftBody* softBody)
{
	appendLinearJoint(specs, clusters[0], softBody);
}

void member_BSoftBody::appendAngularJoint(const AJoint::Specs& specs, Cluster* bodyA, Body bodyB)
{
	AJoint* pj = new AJoint();
	pj->bodies[0] = bodyA;
	pj->bodies[1] = bodyB;
	pj->refs[0] = pj->bodies[0].xform().inverse().mult3(specs.axis);
	pj->refs[1] = pj->bodies[1].xform().inverse().mult3(specs.axis);
	pj->forceMixing = specs.forceMixing;
	pj->factor = specs.factor;
	pj->split = specs.split;
	pj->icontrol = specs.icontrol;
	joints.append(pj);
}
void member_BSoftBody::appendAngularJoint(const AJoint::Specs& specs, Body body)
{
	appendAngularJoint(specs, clusters[0], body);
}
void member_BSoftBody::appendAngularJoint(const AJoint::Specs& specs, BSoftBody* softBody)
{
	appendAngularJoint(specs, clusters[0], softBody);
}

void member_BSoftBody::collisionSoftBody(const SpacePair& pair, BSoftBody* softBody0, BSoftBody* softBody1, BReal mrg, bool useFaceNormal)
{
	Vertex* node = pair.nodeA->value;
	Face* face = pair.nodeB->value;
	BVector bary;
	if (proximityTest(face->vertex0->position, face->vertex1->position, face->vertex2->position, node->position, face->normal, mrg, bary))
	{
		const BVector w = bary;
		const BReal ma = node->imass;
		BReal mb = BaryEval(face->vertex0->imass, face->vertex1->imass, face->vertex2->imass, w);
		if ((face->vertex0->imass <= 0) || (face->vertex1->imass <= 0) || (face->vertex2->imass <= 0))
		{
			mb = 0;
		}
		const BReal ms = ma + mb;
		if (ms > 0)
		{
			FaceVertexContact& contact = softbody_member(softBody0)->faceVertexContacts.append();
			contact.normal = face->normal;
			if (!useFaceNormal && contact.normal.dot(node->position - face->vertex2->position) < 0)
				contact.normal = -face->normal;
			contact.margin = mrg;
			contact.vertex = node;
			contact.face = face;
			contact.bary = w;
			contact.friction = softBody0->friction() * softBody1->friction();
			contact.weights.set(0, 0, 0);
			contact.faceIMass = 0;
			contact.scaleImpulse = 0;
			contact.body = softBody1;
		}
	}
}
void member_BSoftBody::collisionSoftBody(const SpacePair& pair, BSoftBody* softBody0, BSoftBody* softBody1, BReal mrg)
{
	Vertex* vertex = pair.nodeA->value;
	Face* face = pair.nodeB->value;
	BVector o = vertex->position;
	BVector p;
	BReal d = REAL_MAX;
	ProjectOrigin(face->vertex0->position - o, face->vertex1->position - o, face->vertex2->position - o, p, d);
	const BReal m = mrg + (o - vertex->prepos).length() * 2;
	if (d < (m * m))
	{
		const BVector w = BaryCoord(face->vertex0->position, face->vertex1->position, face->vertex2->position, p + o);
		const BReal ma = vertex->imass;
		BReal mb = BaryEval(face->vertex0->imass, face->vertex1->imass, face->vertex2->imass, w);
		if ((face->vertex0->imass <= 0) || (face->vertex1->imass <= 0) || (face->vertex2->imass <= 0))
		{
			mb = 0;
		}
		const BReal ms = ma + mb;
		if (ms > 0)
		{
			SoftContact& contact = softbody_member(softBody0)->soft_contacts.append();
			contact.normal = p / -sqrt(d);
			contact.margin = m;
			contact.node = vertex;
			contact.face = face;
			contact.weights = w;
			contact.friction = bMax(softBody0->friction(), softBody1->friction());
			contact.forceMixing[0] = ma / ms * softbody_member(softBody0)->soft_hardness;
			contact.forceMixing[1] = mb / ms * softbody_member(softBody0)->soft_hardness;
		}
	}
}
void member_BSoftBody::collisionSoftBody(const DbvntPair& pair, BSoftBody* softBody0, BSoftBody* softBody1, BReal margin, bool useFaceNormal)
{
	Face* face1 = pair.a->value;
	Face* face2 = pair.b->value;
	if (face1 != face2)
	{
		Repel(face1, face2, softBody0, softBody1, margin, useFaceNormal);
		Repel(face2, face1, softBody0, softBody1, margin, useFaceNormal);
	}
}
void member_BSoftBody::process(const SpaceNode* la, const SpaceNode* lb, BSoftBody* softBody0, BSoftBody* softBody1, BReal margin, BReal friction)
{
	Cluster* cla = la->value;
	Cluster* clb = lb->value;

	bool connected = false;
	if ((softBody0 == softBody1) && (softbody_member(softBody0)->clusterConnectivity.size()))
	{
		connected = softbody_member(softBody0)->clusterConnectivity[cla->clusterIndex + softbody_member(softBody0)->clusters.size() * clb->clusterIndex];
	}

	if (!connected)
	{
		SoftClusterCollisionShape csa(cla);
		SoftClusterCollisionShape csb(clb);
		IGjkEpaSolver::Result result;
		if (IGjkEpaSolver::SignedDistance(&csa, BMatrix(), &csb, BMatrix(), cla->com - clb->com, result))
		{
			CJoint joint;
			if (softbody_member(softBody0)->SolveContact(result, cla, clb, joint, margin, friction))
			{
				CJoint* pj = new CJoint();
				*pj = joint;
				softbody_member(softBody0)->joints.append(pj);
				pj->factor *= bMax(softbody_member(softBody0)->kSSHR_CL, softbody_member(softBody1)->kSSHR_CL);
				pj->split *= (softbody_member(softBody0)->kSS_SPLT_CL + softbody_member(softBody1)->kSS_SPLT_CL) / 2;
			}
		}
	}
	else
	{
		static int count = 0;
		count++;
	}
}

bool member_BSoftBody::SolveContact(const IGjkEpaSolver::Result& result, Body bodyA, const Body bodyB, CJoint& joint, BReal margin, BReal friction)
{
	if (result.distance < margin)
	{
		BVector normal = result.normal;
		normal.normalize();

		const BVector ra = result.pointA - bodyA.xform().position();
		const BVector rb = result.pointB - bodyB.xform().position();
		const BVector va = bodyA.velocity(ra);
		const BVector vb = bodyB.velocity(rb);
		const BVector vrel = va - vb;
		const BReal rvac = vrel.dot(normal);
		BReal depth = result.distance - margin;

		const BVector iv = normal * rvac;
		const BVector fv = vrel - iv;
		joint.bodies[0] = bodyA;
		joint.bodies[1] = bodyB;
		joint.refs[0] = bodyA.xform().mult3(ra);
		joint.refs[1] = bodyB.xform().mult3(rb);
		joint.rpos[0] = ra;
		joint.rpos[1] = rb;
		joint.forceMixing = 1;
		joint.factor = 1;
		joint.life = 0;
		joint.maxlife = 0;
		joint.split = 1;
		joint.drift = normal * depth;
		joint.normal = normal;
		joint.friction = fv.length2() < (rvac * friction * rvac * friction) ? 1 : friction;
		joint.massmatrix = ImpulseMatrix(bodyA.invMass(), bodyA.invInertia(), joint.rpos[0], bodyB.invMass(), bodyB.invInertia(), joint.rpos[1]);

		return true;
	}
	return false;
}
void member_BSoftBody::Repel(Face* face1, Face* face2, BSoftBody* softBody0, BSoftBody* softBody1, BReal margin, bool useFaceNormal)
{
	for (int node_id = 0; node_id < 3; ++node_id)
	{
		Vertex* vertex = face1->vertex(node_id);
		for (int i = 0; i < 3; ++i)
		{
			if (face2->vertex(i) == vertex)
				return;
		}
	}
	for (int i = 0; i < 3; i++)
	{
		Vertex* vertex = face1->vertex(i);
		BVector bary;
		if (!proximityTest(face2->vertex0->position, face2->vertex1->position, face2->vertex2->position, vertex->position, face2->normal, margin, bary))
			continue;
		FaceVertexContact& contact = softbody_member(softBody0)->faceVertexContacts.append();
		contact.normal = face2->normal;
		if (!useFaceNormal && contact.normal.dot(vertex->position - face2->vertex2->position) < 0)
			contact.normal = -face2->normal;
		contact.margin = margin;
		contact.vertex = vertex;
		contact.face = face2;
		contact.bary = bary;
		contact.friction = softBody0->friction() * softBody1->friction();
		contact.weights.reset();
		contact.faceIMass = 0;
		contact.scaleImpulse = 0;
		contact.body = softBody1;
	}
}

int member_BSoftBody::clusterCount() const
{
	return clusters.size();
}
BVector member_BSoftBody::clusterCom(const Cluster* cluster)
{
	BVector com;
	for (int i = 0; i < cluster->vertexs.size(); ++i)
	{
		com += cluster->vertexs[i]->position * cluster->masses[i];
	}
	return com * cluster->imass;
}

void member_BSoftBody::releaseCluster(int index)
{
	if (Cluster* cluster = clusters.at(index))
	{
		if (cluster->leaf)
		{
			cluster_dbvt.remove(cluster->leaf);
		}
		delete cluster;
		clusters.remove(cluster);
	}
}
void member_BSoftBody::releaseClusters()
{
	while (clusters.size() > 0)
	{
		releaseCluster(0);
	}
}

